%%%%%%%Computing the position at impact through inverse kinematics

function ret = inverse_kin(a)

%%%%Choose the type of inverse kinematics, either numeric or closed form
%%% 1 = numeric solving
%%% 2 = closed form solution (currently only works for non-stance slope and
%%% linearized non-stance slope)
typeofsolve = 1;

if typeofsolve == 1
    
    %%%%% Pick initial guess
    %%%%%q0 as computed from the human parameters
    ret = numeric_inverse_kin(a);
%     q0 = [ -0.5138
%         0.2776
%         0.2870
%         -0.4201
%         0.1746];
%     
%     %     q0 =  [   -0.4248
%     %     0.3006
%     %     0.1242
%     %    -0.3693
%     %     0.1416];
%     
%     %     load('x_opt')
%     %     q0 = x_opt(1:5,:);
%     
%     %%%Numerically solving for the initial condition
%     
%     options = optimset('Display','off');
%     qzero = fsolve(@(q)inverse_kin_fun(q,a),q0,options);
%     
%     ret = qzero;
    
elseif typeofsolve == 2
    
    %%%%If using the old version (no torso) use this:
    
    % theta_a(a)
    
    %%%If using the new version with the torso, use this:
    %%%%(the calculation is broken up, saves a lot of computation time)
    
%     q = zeros(5,1);
    
    ret = math_inverse_kin(a);
    
end


end


function ret = math_inverse_kin(a)
    q = zeros(5,1);
    q(2) = theta_a2(q,a);
    q(5) = theta_a5(q,a);
    q(1) = theta_a1(q,a);
    q(4) = theta_a4(q,a);
    q(3) = theta_a3(q,a);
    
    ret = q;
end
function ret = numeric_inverse_kin(a)
   q0 = q_initial();
   
    %%%Numerically solving for the initial condition
    
    options = optimset('Display','off');
    qzero = fsolve(@(q)inverse_kin_fun(q,a),q0,options);
    
    ret = qzero;
end

function ret = q_initial()
    ret = [ -0.5138
        0.2776
        0.2870
        -0.4201
        0.1746];
end

function F = inverse_kin_fun(q,a)

phipcond = phip_sca(R(q));
y2plus = ya2_vec(R(q), phipcond, a)-yd2_vec(R(q), phipcond,  a);

F = [y2plus;
    h_sca(q)];

end
